
#ifndef PULLEYJOINT_H_
#define PULLEYJOINT_H_

#include "Constraint.h"

namespace p2
{

class PulleyJoint : public Constraint
{
DECLARE_RTT(PulleyJoint, Constraint);

public:
	inline PulleyJoint(Anchor const& a0, Anchor const& pA, real length, bool swapOrder, void* userData);
	inline PulleyJoint(Anchor const& a0, Anchor const& pA, Anchor const& a1, real length, bool swapOrder, void* userData);
	virtual ~PulleyJoint();

	static PulleyJoint* create(Anchor const& a0, Anchor const& pA, real length, bool swapOrder, void* userData);
	static PulleyJoint* create(Anchor const& a0, Anchor const& pA, Anchor const& a1, real length, bool swapOrder, void* userData);

	static Vector2<real> computeTangentPoint(Vector2<real> xPAth, real r2, real order);
	inline real getLength() const;
	inline void setLength(real length);
    inline Anchor const& getPulleyAnchor() const;
    inline void setPulleyAnchor(Anchor const& pA);
    inline real getRadius() const;
    inline void setRadius(real radius);
    inline Vector2<real> getWorldPulleyAnchor0() const;
    inline Vector2<real> getWorldPulleyAnchor1() const;

    /*
     * Constraint implementation
     */
    virtual void preStep(real64 dT);
    virtual bool solvePositions(real64 dT);
    virtual bool solveVelocities(real64 dT);
    virtual void computeFutureAnchorPositions(real64 dT);

private:
    Anchor _pA;

    real _invKpa, _invKpb;

    Vector2<real> _xPth;
    Vector2<real> _rPAth, _rPBth;

    real _radius, _length;

    real _order;
};

inline PulleyJoint::PulleyJoint(Anchor const& a0, Anchor const& pA, real length, bool swapOrder, void* userData)
:	Constraint(a0, pA, userData), _pA(pA), _invKpa(0.f), _invKpb(0.f),
 	_xPth(Vector2<real>::ZERO), _rPAth(Vector2<real>::ZERO), _rPBth(Vector2<real>::ZERO),
 	_radius(0.f), _length(length),
 	_order(swapOrder?-1.f:+1.f)
{
	_radius = _pA.getPoint().length();
}

inline PulleyJoint::PulleyJoint(Anchor const& a0, Anchor const& pA, Anchor const& a1, real length, bool swapOrder, void* userData)
:	Constraint(a0, a1, userData), _pA(pA), _invKpa(0.f), _invKpb(0.f),
 	_xPth(Vector2<real>::ZERO), _rPAth(Vector2<real>::ZERO), _rPBth(Vector2<real>::ZERO),
 	_radius(0.f), _length(length),
 	_order(swapOrder?-1.f:+1.f)
{
	_radius = _pA.getPoint().length();
}

inline real PulleyJoint::getLength() const
{
	return _length;
}

inline void PulleyJoint::setLength(real length)
{
	_length = length;
}

inline Anchor const& PulleyJoint::getPulleyAnchor() const
{
	return _pA;
}

inline void PulleyJoint::setPulleyAnchor(Anchor const& pA)
{
	_pA = pA;
}

inline real PulleyJoint::getRadius() const
{
	return _radius;
}

inline void PulleyJoint::setRadius(real radius)
{
	_radius = radius;
}

inline Vector2<real> PulleyJoint::getWorldPulleyAnchor0() const
{
	return _xPth + _rPAth;
}

inline Vector2<real> PulleyJoint::getWorldPulleyAnchor1() const
{
	return _xPth + _rPBth;
}

} /* namespace p2 */
#endif /* PULLEYJOINT_H_ */
